#pragma once

#include "vector.h"
#include "matrix.h"

namespace g2 {

template <typename T>
class basic_perspective;

typedef basic_perspective<double> perspective;
typedef basic_perspective<float> perspectivef;
typedef basic_perspective<long double> perspectivel;

// Perspective transform of the plane
template <class T>
class basic_perspective
{
public:
	typedef T element_type;
public:
	// p_new = (a(p_old) + b) / (1 + c^T p_old)
	basic_matrix<T> a;
	basic_vector<T> b, c; 
public:
	basic_perspective() 
		{ }
	template <class U>
	basic_perspective(basic_matrix<U> const & a, basic_vector<U> const & b, basic_vector<U> const & c) : a(a), b(b), c(c)
		{ }
	basic_perspective & identity() 
		{ a.identity(); b.zero(); c.zero(); return * this; }

	friend bool operator == (const basic_perspective<T> & lhs, const basic_perspective<T> & rhs) 
		{ return lhs.a == rhs.a && lhs.b == rhs.b && lhs.c == rhs.c; }
	friend bool operator != (const basic_perspective<T> & lhs, const basic_perspective<T> & rhs) 
		{ return !(lhs == rhs); }

	basic_vector<T> operator ()(basic_vector<T> const & p_old) const 
		{ return (a(p_old) + b) * (1 / (1 + scalar_product(c, p_old))); };
	basic_vector<T> inverse(basic_vector<T> const & p_new) const
		{ return (a - tproduct(p_new, c)).inverse(p_new - b); }

	basic_perspective<T> inverse() const;

  /// modifies the transformation so that it remains valid after scaling of all points in s times
  basic_perspective & scale(T s)
    { b *= s; c *= (1 / s); return * this; }

  static const char * name() { return "perspective"; }
};

template <class T>
basic_perspective<T> basic_perspective<T>::inverse() const 
{
	basic_matrix<T> i = a.inverse();
  basic_vector<T> e = -i(b);
  basic_vector<T> f = -c * i;
	return basic_perspective<T>(
    (1 + scalar_product(c, e)) * i + tproduct(e, f), e, f);
}

template <class T>
basic_perspective<T> operator * (const basic_perspective<T> & x, const basic_perspective<T> & y) 
{
  T d = 1 / (1 + scalar_product(x.c, y.b));
  return basic_perspective<T>(
    d * (x.a * y.a + tproduct(x.b, y.c)),
    d * (x.a(y.b) + x.b),
    d * (x.c * y.a + y.c));
}

// test

template <class T>
void test_perspective()
{
  basic_perspective<T> p(
    basic_matrix<T>(1.0, 0.99, 1.01, 1.02),
    basic_vector<T>(0.3, 0.4),
    basic_vector<T>(0.03, -0.04)
  );

  // test for transformation inversion
  basic_perspective<T> i = p.inverse();
  basic_perspective<T> ii = i.inverse();
  // check here p == ii

  basic_perspective<T> q(
    basic_matrix<T>(0.7, 0.99, 0.99, 0.8),
    basic_vector<T>(-0.3, -0.1),
    basic_vector<T>(-0.03, 0.04)
  );

  basic_vector<T> x1(1, 2);
  basic_vector<T> x2(-1.1, 2.1);

  // test for transformations composition
  basic_vector<T> a1 = p(q(x1));
  basic_vector<T> a2 = p(q(x2));
  basic_vector<T> b1 = (p*q)(x1);
  basic_vector<T> b2 = (p*q)(x2);
  // check here a1 == b1 && a2 == b2

  // test for point inverse
  a1 = p.inverse(x1);
  a2 = p.inverse(x2);
  b1 = i(x1);
  b2 = i(x2);
  // check here a1 == b1 && a2 == b2
}

} //namespace g2
